#include "libServo.h"
#include "include/cJSON.h"
#include <stdio.h>
#include <malloc.h>
#include <unistd.h>
#include "wiringPi.h"
#include "pthread.h"
#include "libServoShm.h"
#include "string.h"
#define configfile "/app/config/Servoconfig.json"
#define filesize 1024*5

#define PWMPIN1 1
#define PWMPIN2 26
#define PWMPIN3 23
#define PWMPIN4 24




char filebuff[filesize]={0};
int intServo_pin=-1;

#define step_maxnum 10
typedef struct {
   int Servoport;
}SERVO_DATA; //舵机初始化数据
typedef struct {
    int enable;  //使能
    int deV_num; //步进电机编号
    int dev_ping;
    int en_ping;
    int dir_ping;
    int dir_count;  //驱动器配置的多少步一圈
    int dev_busy; //忙碌
    pthread_t Stepper_thread;
    pthread_mutex_t lock ; //互斥锁
    pthread_cond_t cond ;//条件变量
}STEP_DATA; //步进电机初始化数据
typedef struct {
    SERVO_DATA servoData;
    STEP_DATA stepData[step_maxnum];
}CONFIG_DATA;//所有的初始化数据
CONFIG_DATA configData={0};
int Servo_init(void) {
    printf("Servo_init!\n");

    int  pwmpin=configData.servoData.Servoport;
    if((pwmpin!=PWMPIN1)&&(pwmpin!=PWMPIN2)&&(pwmpin!=PWMPIN3)&&(pwmpin!=PWMPIN4)){
        printf("[ERROR] illegal Servoport: %d\n",pwmpin);
        return -1;
    }
    intServo_pin = pwmpin;
    printf("[Servo]  Servoport: %d\n",pwmpin);
    wiringPiSetup ();
    pinMode (pwmpin, PWM_OUTPUT) ;
    pwmSetMode(PWM_MODE_MS);
    pwmSetRange(1000);//19.2MHz /
    pwmSetClock(192*2);//50hz

    pwmWrite (pwmpin, 25); //默认为0度
    return 0;
}
int Servo_SetAngle(int Angle){
    int ot_angle =0;
   if(intServo_pin==-1){
       printf("[ERROR] Servo_init fail\n");
       return -1;
   }
   if(Angle<=0) ot_angle=25;
   else if(Angle>=180) ot_angle=125;
   else ot_angle=25+Angle*100/180;
    pwmWrite (intServo_pin, ot_angle); //默认为0度
    return 0;
}
//释放信号量 让命令执行
int Step_Cmd_Execute(int step_num){
    for (int i = 0; i <step_maxnum ; ++i) {
        if(configData.stepData[i].deV_num==step_num){
            if(configData.stepData[i].dev_busy ==1) return -1;
            pthread_mutex_lock(&configData.stepData[i].lock);
            configData.stepData[i].dev_busy = 1;
            pthread_cond_signal(&configData.stepData[i].cond); // 唤醒等待的线程
            pthread_mutex_unlock(&configData.stepData[i].lock);
            return 0;

        }

    }
    return -1;
}

// 线程函数
void *Step_thread_call(void *arg){
    STEP_DATA  *thread_data  =(STEP_DATA*)arg;
    int ret =0;
    int data,flag;
    if(thread_data->dev_ping==255)
        return NULL;
    if(thread_data->dir_ping==255)
        return NULL;
    pinMode(thread_data->dev_ping, OUTPUT);//初始化脉冲引脚
    pinMode(thread_data->dir_ping, OUTPUT);//初始化方向引脚
    void * handle = Get_Shm_Pointer();
    int speedlist[5]={100,200,500,1000,5000};
    if(thread_data->en_ping!=255){
        pinMode(thread_data->en_ping, OUTPUT);
        printf("[step %d] enping set %d\n",thread_data->deV_num,thread_data->enable);
        digitalWrite (thread_data->en_ping, !thread_data->enable) ;//这里要根据实际的高电平使能低电平是能来操作 我这里是低电平使能所以取反
    }

    if(handle==NULL){
        printf("[ERROR] Get_Shm_Pointer return NULL\n");
        return NULL;
    }


//    printf("Function: %s, Line: %d debug %d  %d step=%d\n",__FUNCTION__, __LINE__,data,flag,thread_data->deV_num);
//    digitalWrite (thread_data->dir_ping, 0) ; //方向控制
    while(1){
        pthread_mutex_lock(&thread_data->lock);
        while (thread_data->dev_busy!=1) {
            // 等待 dev_busy 变为 1
            pthread_cond_wait(&thread_data->cond, &thread_data->lock);
        }
        // ready 现在是 1，可以执行后续操作
        pthread_mutex_unlock(&thread_data->lock);
       // printf("[step %d]Command execution!\n",thread_data->deV_num);


        Read_Data_pram(handle,stepshm_X(thread_data->deV_num)+S_mode,&data,&flag);
        printf("[step=%d] mode=%d ",thread_data->deV_num,data);
        int count =0;
        if(data==1){//圈数
            ret = Read_Data_pram(handle,stepshm_X(thread_data->deV_num)+S_circle,&data,&flag);
            if(ret==0&&flag==1){
                count =data*thread_data->dir_count;
            }
        }else if(data==2){//步数
            ret = Read_Data_pram(handle,stepshm_X(thread_data->deV_num)+S_step,&data,&flag);
            if(ret==0&&flag==1){
                count =data;
            }
        }
        printf("count=%d ",count);
        ret = Read_Data_pram(handle,stepshm_X(thread_data->deV_num)+S_dir,&data,&flag);
        if(ret==0){
            digitalWrite (thread_data->dir_ping, data) ; //方向控制
        }
        printf("dir=%d ",data);
        int speed =0;
        ret = Read_Data_pram(handle,stepshm_X(thread_data->deV_num)+S_speed,&data,&flag);
        if(ret==0){
            speed = data;
        }
        if(speed<0||speed>4) speed=3;
        printf("speed=%d \n",data);
        while (count--){
            digitalWrite (thread_data->dev_ping, 1) ;
            usleep(speedlist[speed]);//5us
            digitalWrite (thread_data->dev_ping, 0) ;
            usleep(speedlist[speed]);//5us
        }

        usleep(500000);//等待500ms
        pthread_mutex_lock(&thread_data->lock);
        thread_data->dev_busy=0;
        pthread_mutex_unlock(&thread_data->lock);

        printf("action end \n");

    }
    pthread_exit(NULL);



}


int Step_init(void){
    printf("Step_init!\n");
    int re=0;
    for (int i = 0; i <step_maxnum; ++i) {
        if(configData.stepData[i].enable==1){
            printf("[Step %d]  enable=%5d dev_num=%5d dev_ping=%5d en_ping=%5d dir_ping=%5d dir_count=%5d\n",i,configData.stepData[i].enable,configData.stepData[i].deV_num,configData.stepData[i].dev_ping,
                   configData.stepData[i].en_ping,configData.stepData[i].dir_ping,configData.stepData[i].dir_count);
            re = pthread_create(&configData.stepData[i].Stepper_thread, NULL, Step_thread_call,&configData.stepData[i]);
            if (re != 0) {
                printf("Error: pthread_create() failed\n");
                return -1;
            }
        }

    }

    return 0;
}
/**
 *步进电机和舵机初始化
 * */
int motion_init(void){

    printf("motion_init!\n");
    FILE *fp;
    fp = fopen(configfile,"r");
    int re =0;
    if(fp<=0){
        printf("[ERROR] %s open fail!\n",configfile);
        return -1;
    }
    re=(int)fread(filebuff,1,filesize,fp);
    if(re<=0){
        printf("[ERROR] %s fread fail!\n",configfile);
        return -1;
    }
    fclose(fp);
///=============================================================开始解析json=================================================
    cJSON* cjson_main =  cJSON_Parse(filebuff);
    cJSON* cjson_Step =  cJSON_GetObjectItem(cjson_main, "Step");
    cJSON* cjson_Servoport =  cJSON_GetObjectItem(cjson_main, "Servoport");
    configData.servoData.Servoport=cjson_Servoport->valueint;

    cJSON* cjson_dev_list =  cJSON_GetObjectItem(cjson_Step,"dev_list");
    int dev_list_array_size = cJSON_GetArraySize(cjson_dev_list);

    for (int i=0;i<dev_list_array_size;i++){

        cJSON* cjson_skill_item = cJSON_GetArrayItem(cjson_dev_list, i);

        cJSON* cjson_dev_num =  cJSON_GetObjectItem(cjson_skill_item,"dev_num");
        configData.stepData[i].deV_num = cjson_dev_num->valueint;
        cJSON* cjson_dev_ping =  cJSON_GetObjectItem(cjson_skill_item,"dev_ping");
        configData.stepData[i].dev_ping = cjson_dev_ping->valueint;
        cJSON* cjson_dir_ping =  cJSON_GetObjectItem(cjson_skill_item,"dir_ping");
        configData.stepData[i].dir_ping = cjson_dir_ping->valueint;
        cJSON* cjson_en_ping =  cJSON_GetObjectItem(cjson_skill_item,"en_ping");
        configData.stepData[i].en_ping = cjson_en_ping->valueint;

        configData.stepData[i].enable=1;
        configData.stepData[i].dir_count=800;

    }
///=============================================================解析json结束=================================================

    re = Init_ServoShm();
    if(re==-1){
        printf("[ERROR] Servo_init!\n");
        return -1;
    }
    re = Servo_init();
    if(re==-1){
        printf("[ERROR] Servo_init fail!\n");
        return -1;
    }
    re =Step_init();
    if(re==-1){
        printf("[ERROR] Step_init fail!\n");
        return -1;
    }


}